#include <stdio.h>
#include <math.h>
#include <iostream>
#include <sys/types.h>
#include <sys/shm.h>
#include <libplayerc++/playerc++.h>
#include "defs.h"

typedef struct 
{
   double coordenada_x;
   double coordenada_y;
} produto_cartesiano;

int main(int argc, char *argv[])
{
  using namespace PlayerCc;

  PlayerClient robot("localhost");
  Position2dProxy pp(&robot,0);
  LaserProxy lp(&robot,0);
  MapProxy mp(&robot,0);

  float velocidade, ratacao, aX, aY, aYaw, aGx, aGy, aGa;
  int i, j, numero_vertices, quadrados_restantes;
  int *matriz_custos_linearizada;
  char *caminho;
  double ponto_central, v_x, v_y, w;

  //Calibra os valores das coordenadas do Robo
  double x = 0, x_mapa, target_x, target_x2 = 0, error_x = 0;
  double y = 0, y_mapa, target_y, target_y2 = 0, error_y = 0;
  double theta = 0, target_theta, target_theta2 = 0, error_theta = 0;

	int shmid;
	char* shm, *s;
	key_t key = KEY_SHDMEM;

	// Cria area de memoria compartilhada (create & read-write)
	if ((shmid = shmget(key, sizeof(int)*TAMANHO_CUSTOS*TAMANHO_CUSTOS, 0666)) == -1)
	{
		perror("shmget"); 
		exit(-1);
	}

	// Anexa o processo corrente a area recem criada
	if ((shm = (char*) shmat(shmid, NULL, 0)) == (char *) -1) 
	{
		perror("shmat");
		exit(-1);
	}

	numero_vertices = 10;
	caminho = (char*) malloc(sizeof(char)*numero_vertices);

	s = shm;
	for(i = 0; i < numero_vertices; i++)
		caminho[i] = *s++;

  produto_cartesiano quadrados[TAMANHO_CUSTOS];
  
  ponto_central = (TAMANHO_OCUPACAO/2.0) - 1;
  
  for (i = 0; i < TAMANHO_CUSTOS; i++)
  {
      quadrados[i].coordenada_x = (((i % TAMANHO_OCUPACAO) - ponto_central) - 0.5);
      quadrados[i].coordenada_y = (((i / TAMANHO_OCUPACAO) - ponto_central) - 0.5);
  }
  
  // usado para definir um número máximo de iterações	uint w = mp.GetHeight();
  int iter = 0;
  char sentido = 'T';

  robot.Read();

  //Lê a localização do Robo
  pp.SetOdometry(x, y, theta); 
//  printf("x = %f y = %f theta = %f\n", x, y, theta);
  
  //Atualiza os valores
  do {
	  //Lê todos os parametros do robo
	  robot.Read();

  } while((pp.GetXPos() != x) || (pp.GetYPos() != y) || (pp.GetYaw() != theta));
 
  quadrados_restantes = numero_vertices - 2;
  while (quadrados_restantes >= 0)
  {
    target_theta2 = 0;
    target_x2 = quadrados[caminho[quadrados_restantes] - 1].coordenada_x;
    target_y2 = quadrados[caminho[quadrados_restantes] - 1].coordenada_y;

//	printf("\nSentido: %c || X = %f Target_x = %f || Y = %f Target_y = %f", sentido, x_mapa, target_x2, y_mapa, target_y2);
    
    if (quadrados_restantes == numero_vertices - 2)
    {
      x_mapa = quadrados[caminho[quadrados_restantes + 1] - 1].coordenada_x;
      y_mapa = quadrados[caminho[quadrados_restantes + 1] - 1].coordenada_y;
      
      if (x_mapa != target_x2)
	sentido = 'X';
      else if (y_mapa != target_y2)
	sentido = 'Y';
    }
    else
    {    
      if ((x_mapa != target_x2 && sentido == 'X'))
	target_theta2 = 0;
      else if ((y_mapa != target_y2 && sentido == 'Y'))
	target_theta2 = 0;
      else if (x_mapa != target_x2 && sentido == 'Y')
      {
	if (quadrados[caminho[quadrados_restantes - 1] - 1].coordenada_y >= target_y2)
	{
	  if (target_x2 >= x_mapa)
	    target_theta2 = PI/2.0;
	  else
	    target_theta2 = -PI/2.0;
	}
	else
	{
	  if (target_x2 >= x_mapa)
	    target_theta2 = -PI/2.0;
	  else
	    target_theta2 = PI/2.0;
	}

      }
      else if (y_mapa != target_y2 && sentido == 'X')
      {
	if (quadrados[caminho[quadrados_restantes - 1] - 1].coordenada_x >= target_x2)
	{
	  if (target_y2 >= y_mapa)
	    target_theta2 = PI/2.0;
	  else
	    target_theta2 = -PI/2.0;
	}
	else
	{
	  if (target_y2 >= y_mapa)
	    target_theta2 = -PI/2.0;
	  else
	    target_theta2 = PI/2.0;
	}
      }
      else
	target_theta2 = 0;    
    }
        x_mapa = quadrados[caminho[quadrados_restantes] - 1].coordenada_x;
        y_mapa = quadrados[caminho[quadrados_restantes] - 1].coordenada_y;

//	printf("\n(%f,%f,%f) || (%f,%f,%f)", target_x, target_y, target_theta , target_x2, target_y2, target_theta2);
	//getchar();
	if (target_theta2 == 0)
	{
		target_x = -target_x2 / 1.5 + x;
		target_y = -target_y2 / 1.5 + y;  
	}
	else
	{
		target_x = x;
		target_y = y;
		target_theta += target_theta2;
	}


    
     do {
	error_x = target_x - x;
	error_y = target_y - y;
	error_theta = target_theta - theta;
	    
	v_x = error_x * cos(target_theta) + error_y * sin(target_theta);
	v_y = - error_x * sin(target_theta) + error_y * cos(target_theta);
	w = error_theta;
    
	pp.SetSpeed(v_x, v_y, w);
	robot.Read();
  
	x = pp.GetXPos();
	y = pp.GetYPos();
	theta = pp.GetYaw();
  
      } while((iter++ < 100));// && (fabs(error_theta) == 0.0)));  	
      iter = 0;
	if (target_theta2 != 0)
	{
		if (sentido == 'X') sentido = 'Y';
		else sentido = 'X';
	}
	else
      		quadrados_restantes--;
    }
      
  return 0;
}

